// Ted Carancho's MikroQuad - September 2008
// An Arudino based quadrocopter using the Sparkfun
// 5DOF IMU and the Pololu RC servo controller.

#include <stdlib.h>

//Debug data
#define DEBUG 1
#define BAUD 38400

// Define pin assignments
#define LEDPIN 13
#define RESETPIN 12
#define THROTTLEPIN 2
#define ROLLPIN 3
#define PITCHPIN 4
#define YAWPIN 5
#define GEARPIN 6
#define AUXPIN 7
#define XACCELPIN 0
#define YACCELPIN 1 
#define ZACCELPIN 2 
#define XRATEPIN 4 
#define YRATEPIN 3 

// Define servo assignments
#define FRONTCHANNEL 0
#define RIGHTCHANNEL 1
#define REARCHANNEL 2
#define LEFTCHANNEL 3

// Define transmitter commands
#define SEARCHTIMEOUT 25000
#define TIMEOUT 4500
int throttle = 0;
int roll = 0;
int pitch = 0;
int yaw = 0;
int gear = 0;
int aux = 0;
float radConvert = PI / 1000;
int limitStick = 1; // make transmitter stick less sensitive
byte xmitCheck = 0;

// Define motor commands
unsigned int frontCommand = 0;
unsigned int rightCommand = 0;
unsigned int rearCommand = 0;
unsigned int leftCommand = 0;
unsigned int throttleMotorCommand = 0;
int rollMotorCommand = 0;
int pitchMotorCommand = 0;
int yawMotorCommand = 0;

// Accelerometer setup
// these A/D values depend on how well the sensors are mounted
// change these values to your unique configuration
#define XMIN 405
#define XMAX 607
#define YMIN 409
#define YMAX 618
#define ZMIN 403
#define ZMAX 611
float xAccMid = ((float)(XMAX-XMIN) / 2.0) + (float)XMIN;
float yAccMid = ((float)(YMAX-YMIN) / 2.0) + (float)YMIN;
float zAccMid = ((float)(ZMAX-ZMIN) / 2.0) + (float)ZMIN;
float xAccScaleFactor = 1.0 / (float)(XMAX - XMIN) * 2.0;
float yAccScaleFactor = 1.0 / (float)(YMAX - YMIN) * 2.0;
float zAccScaleFactor = 1.0 / (float)(ZMAX - ZMIN) * 2.0;
int xAccZero, yAccZero, zAccZero;

// Gyro setup
// if AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
// gyro range is from +/-500 deg/sec
float GyroMid = 465;
float GyroScaleFactor = 1001 / (float)ZMAX * 0.01745328;
int xGyroZero, yGyroZero;
float PiOver2 = PI / 2;

// Kalman filter
float tmp, roll_angle, pitch_angle;
float acc_gyro = 0, dt, accZ;
struct Gyro1DKalman {
	float x_angle, x_bias;
	float P_00,  P_01, P_10, P_11;	
	float Q_angle, Q_gyro;
	float R_angle;
};
struct Gyro1DKalman filter_roll;
struct Gyro1DKalman filter_pitch;

unsigned long previousTime = 0;
unsigned long currentTime = 0;

float pgain = 500;
float igain = 0.1;
float dgain = -2000;
float iRollState = 0;
float lastRollPosition = 0;
float iPitchState = 0;
float lastPitchPosition = 0;


void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  pinMode(LEDPIN, OUTPUT);
  pinMode(RESETPIN, OUTPUT);
  
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  init_Gyro1DKalman(&filter_roll, 0.0001, 0.0003, 0.3);
  init_Gyro1DKalman(&filter_pitch, 0.0001, 0.0003, 0.3);
  previousTime = millis();
  digitalWrite(LEDPIN, LOW);
  
  /*if (DEBUG) {
    // temporarily force manual input of this until right PID values are found
    Serial.println("Enter P value. (too high makes unstable) ");
    pgain = readSerial();
    Serial.println("Enter I value. (reset, remove offset, add D if oscillates)");
    igain = readSerial();
    Serial.println("Enter D value. (pre-act, add gain & reset)");
    dgain = readSerial();
  }*/
  
  //Serial.print((int)pgain); Serial.print(","); Serial.print((int)(igain*10)); Serial.print(","); Serial.println((int)dgain);
  
  digitalWrite(RESETPIN, LOW);
  delay(1000);
  digitalWrite(RESETPIN, HIGH);
  
  //Provide time for ESC's to initialize
  xAccZero = xAccMid - findZero(XACCELPIN);
  yAccZero = yAccMid - findZero(YACCELPIN);
  zAccZero = ZMAX - findZero(ZACCELPIN);
  xGyroZero = GyroMid - findZero(XRATEPIN);
  yGyroZero = GyroMid - findZero(YRATEPIN);
  delay(5000);
  for (byte channel = 0; channel < 4; channel++) motorConfigure(channel);
  digitalWrite(LEDPIN, HIGH);
  attachInterrupt(1, readTransmitter, RISING);
}

void loop() {

  currentTime = millis();
  dt = (float)(currentTime - previousTime) / 1000.0;
  previousTime = currentTime;

  // Kalman Filter 
  accZ = accelero_z();

  tmp = PredictAccG_pitch(-accelero_x(), accZ);
  ars_predict(&filter_pitch, gyro_pitch_rad(), dt); // Kalman predict
  pitch_angle = ars_update(&filter_pitch, tmp);     // Kalman update + result (angle) 

  tmp = PredictAccG_roll(accelero_y(), accZ);
  ars_predict(&filter_roll, gyro_roll_rad(), dt); // Kalman predict
  roll_angle = ars_update(&filter_roll, tmp);     // Kalman update + result (angle) 

  // Read transmitter commands
  // Range is from 1000-2000
  /*roll = pulseIn(ROLLPIN, HIGH, SEARCHTIMEOUT) / limitStick;
  pitch = pulseIn(PITCHPIN, HIGH, TIMEOUT) / limitStick;
  throttle = pulseIn(THROTTLEPIN, HIGH, TIMEOUT) / limitStick;
  yaw = pulseIn(YAWPIN, HIGH, TIMEOUT) / limitStick;*/

  // PID controller
  rollMotorCommand = (int)(updatePID((roll - 1500.0) * radConvert, roll_angle, &lastRollPosition, &iRollState));
  pitchMotorCommand = (int)(updatePID(-(pitch - 1500.0) * radConvert, pitch_angle, &lastPitchPosition, &iPitchState));
  
  // Calculate motor commands
  // Range for Pololu RC servo controller is 2000-4700
  throttleMotorCommand = (constrain((throttle - 1000), 0, 1000) * 2.7) + 2000;
  yawMotorCommand = (constrain((yaw - 1500), -500, 500) * 2.7);
   
  frontCommand = constrain((unsigned int)(throttleMotorCommand + pitchMotorCommand + yawMotorCommand), 2000, 4700);
  rearCommand = constrain((unsigned int)(throttleMotorCommand - pitchMotorCommand + yawMotorCommand), 2000, 4700);
  rightCommand = constrain((unsigned int)throttleMotorCommand + rollMotorCommand - yawMotorCommand, 2000, 4700);
  leftCommand = constrain((unsigned int)(throttleMotorCommand - rollMotorCommand - yawMotorCommand), 2000, 4700);
  
  // Command motors
  if (DEBUG) {
    // xmitData();
    // test one axis
    motorCommand(FRONTCHANNEL, frontCommand);
    motorCommand(REARCHANNEL, rearCommand);
  }
  else {
    motorCommand(FRONTCHANNEL, frontCommand);
    motorCommand(REARCHANNEL, rearCommand);
    motorCommand(RIGHTCHANNEL, rightCommand);
    motorCommand(LEFTCHANNEL, leftCommand);
  }
}
